#include <string.h>
#include "util_mem.h"

#if UTILMEM_CFG_INSTANCE_NUM

#if UTILMEM_CFG_BYTE_ALIGN == 0
    #error "UTILMEM_CFG_BYTE_ALIGN must be greater than 0"
#endif

#if ((UTILMEM_CFG_NAME_MAX_LEN < 4) || (UTILMEM_CFG_NAME_MAX_LEN > 32))
    #error "UTILMEM_CFG_NAME_MAX_LEN must be between 4 and 32"
#endif

#define UTILMEM_MININUM_BLOCK_SIZE ((size_t)(sc_util_mem_block_size << 1))

struct UtilMemBlock {
    struct UtilMemBlock *next;
    size_t size;
};

struct UtilMemInstance {
    uint8_t *physical_block;                /* The argument(block) to the UtilMemCreate */
    size_t physical_block_size;             /* The argument(block_size) to the UtilMemCreate */
    char name[UTILMEM_CFG_NAME_MAX_LEN];    /* The argument(name) to the UtilMemCreate, unique */
    uint8_t *logic_block;                   /* After aligned */
    size_t history_min_size;                /* History free size minimum */
    size_t current_size;                    /* Current free size */
    struct UtilMemBlock block_start;
    struct UtilMemBlock *block_end;
};

static const size_t sc_util_mem_block_size = (sizeof(struct UtilMemBlock) + (UtilPointSizeType)(UTILMEM_CFG_BYTE_ALIGN - 1)) & ~(UtilPointSizeType)(UTILMEM_CFG_BYTE_ALIGN - 1);
static uint16_t s_util_mem_instance_num = 0;
static struct UtilMemInstance *s_util_mem_instance = NULL;

static void initUtilMemInstance(struct UtilMemInstance *instance);
static int8_t searchUtilMemInstance(const char *name);
static void copyNameToArray(char *dst, const char *src);
static void initUtilMemBlock(struct UtilMemInstance *instance);
static void insertBlockToFreeList(struct UtilMemInstance *instance, struct UtilMemBlock *new_block);
static int8_t regionWillBeCoverd(uint8_t *block, size_t block_size);

uint8_t UtilMemCreate(uint8_t *block, size_t block_size, const char *name)
{
    /* util_mem_instance is All mem instance */
    static struct UtilMemInstance util_mem_instance[UTILMEM_CFG_INSTANCE_NUM];

    int8_t index;
    char name_array[UTILMEM_CFG_NAME_MAX_LEN];

    /* Check if s_util_mem_instance is initialized */
    if (s_util_mem_instance == NULL) {
        initUtilMemInstance(util_mem_instance);
    }

    /* Avoid this situation:
     * The length of the name exceeds the limit, resulting in a matching failure.
     * But when create a new instance, it has the same name because clipped by the limit length */
    copyNameToArray(name_array, name);
    index = searchUtilMemInstance(name_array);

    /* If the name is already used, return UTILMEM_RETURN_NAME_EXIST */
    if (index >= 0) {
        UtilLogW(__FUNCTION__, "UTILMEM_RETURN_NAME_EXIST");
        return UTILMEM_RETURN_NAME_EXIST;
    }
    else {
        UtilLogI(__FUNCTION__, "Create instance: %s", name_array);
    }

    /* If the quantity has reached the upper limit, return UTILMEM_RETURN_INSTANCE_FULL */
    if (s_util_mem_instance_num >= UTILMEM_CFG_INSTANCE_NUM) {
        UtilLogW(__FUNCTION__, "UTILMEM_RETURN_INSTANCE_FULL");
        return UTILMEM_RETURN_INSTANCE_FULL;
    }
	
	if (regionWillBeCoverd(block, block_size) != 0) {
		UtilLogW(__FUNCTION__, "UTILMEM_RETURN_REGION_COVER");
		return UTILMEM_RETURN_REGION_COVER;
	}

    s_util_mem_instance[s_util_mem_instance_num].physical_block = block;
    s_util_mem_instance[s_util_mem_instance_num].physical_block_size = block_size;
    copyNameToArray(s_util_mem_instance[s_util_mem_instance_num].name, name_array);
    initUtilMemBlock(&s_util_mem_instance[s_util_mem_instance_num]);
    s_util_mem_instance_num++;

    return UTILMEM_RETURN_OK;
}

void *UtilMemMalloc(const char *name, size_t size)
{
    int8_t index;
    size_t addtional_size;
    struct UtilMemBlock *block;
    struct UtilMemBlock *previous_block;
    struct UtilMemBlock *new_block;

    void *ret = NULL;

    index = searchUtilMemInstance(name);
    if (index < 0) {
        UtilLogW(__FUNCTION__, "Instance not found: %s", name);
        return NULL;
    }

    if (size == 0) {
        UtilLogI(__FUNCTION__, "size == 0");
        return NULL;
    }

    UtilLogI(__FUNCTION__, "Source size = %d", size);
    addtional_size = sc_util_mem_block_size + UTILMEM_CFG_BYTE_ALIGN - (size & (UTILMEM_CFG_BYTE_ALIGN - 1));
    size += addtional_size;
    UtilLogI(__FUNCTION__, "Addtional size = %d, total size = %d", addtional_size, size);

    if (size > s_util_mem_instance[index].current_size) {
        UtilLogI(__FUNCTION__, "Don't have enough memory");
        return NULL;
    }

    previous_block = &s_util_mem_instance[index].block_start;
    block = previous_block;
    while ((block->size < size) && (block->next != NULL)) {
        previous_block = block;
        block = block->next;
    }

    if (block == s_util_mem_instance[index].block_end) {
        UtilLogW(__FUNCTION__, "Total memory is suffcient, but every blocks don't have wanted size");
        return NULL;
    }

    UtilLogI(__FUNCTION__, "Get block: 0x%p, size = %d", (void *)block, block->size);

    ret = (void *)((uint8_t *)previous_block->next + sc_util_mem_block_size);
    previous_block->next = block->next;
    UtilLogI(__FUNCTION__, "Return address: 0x%p", ret);

    if ((block->size - size) > UTILMEM_MININUM_BLOCK_SIZE) {
        new_block = (void *)((uint8_t *)block + size);
        new_block->size = block->size - size;
        block->size = size;
        UtilLogI(__FUNCTION__, "new_block address: 0x%p, size: %d", new_block, new_block->size);

        insertBlockToFreeList(&s_util_mem_instance[index], new_block);
    }
    s_util_mem_instance[index].current_size -= block->size;
    if (s_util_mem_instance[index].current_size < s_util_mem_instance[index].history_min_size) {
        s_util_mem_instance[index].history_min_size = s_util_mem_instance[index].current_size;
    }
    block->next = NULL;

    return ret;
}

void *UtilMemCalloc(const char *name, size_t nitems, size_t size)
{
    void * pv = UtilMemMalloc(name, nitems * size);

    if (pv != NULL) {
        memset(pv, 0, nitems * size);
    }
	
	return pv;
}

void UtilMemFree(const char *name, void *pv)
{
    int8_t index;
    uint8_t *puc;
    struct UtilMemBlock *block;

    if (pv == NULL) {
        UtilLogI(__FUNCTION__, "pv == NULL");
        return;
    }

    index = searchUtilMemInstance(name);
    if (index == -1) {
        UtilLogW(__FUNCTION__, "Instance %s not found", name);
        return;
    }

    puc = (uint8_t *)pv;
    puc -= sc_util_mem_block_size;
    block = (void *)puc;
    if (block->next != NULL) {
        UtilLogE(__FUNCTION__, "MemBlock error, next != NULL");
        return;
    }

    s_util_mem_instance[index].current_size += block->size;
    insertBlockToFreeList(&s_util_mem_instance[index], block);
}

/**
 * @brief   Initialize UtilMemInstance
 */
static void initUtilMemInstance(struct UtilMemInstance *instance)
{
    uint32_t i, j;

    for (i = 0; i < UTILMEM_CFG_INSTANCE_NUM; i++) {
        instance[i].physical_block = NULL;
        instance[i].physical_block_size = 0;
        for (j = 0; j < UTILMEM_CFG_NAME_MAX_LEN; j++) {
            instance[i].name[j] = '\0';
        }
        instance[i].logic_block = NULL;
        instance[i].physical_block_size = 0;
        instance[i].history_min_size = 0;
        instance[i].current_size = 0;
    }

    s_util_mem_instance = instance;
}

/**
 * @brief   Search UtilMemInstance by name
 * @returns index of UtilMemInstance. If not found, return -1
 */
static int8_t searchUtilMemInstance(const char *name)
{
    int8_t i;

    if (name == NULL) {
        UtilLogW(__FUNCTION__, "name == NULL");
        return -1;
    }

    for (i = 0; i < UTILMEM_CFG_INSTANCE_NUM; i++) {
        if (strcmp(s_util_mem_instance[i].name, name) == 0) {
            return i;
        }
    }

    return -1;
}

/**
 * @brief   Copy name to array
 */
static void copyNameToArray(char *dst, const char *src)
{
    uint32_t i;

    for (i = 0; i < UTILMEM_CFG_NAME_MAX_LEN; i++) {
        dst[i] = src[i];
        if (src[i] == '\0') {
            break;
        }
    }
}

static void initUtilMemBlock(struct UtilMemInstance *instance)
{
    UtilPointSizeType block_address = (UtilPointSizeType)instance->physical_block;
    size_t block_size = instance->physical_block_size;

    UtilLogI(__FUNCTION__, "block_address = 0x%p, block_size = %d", (void *)block_address, block_size);

    /* Align the block address */
    if ((block_address % UTILMEM_CFG_BYTE_ALIGN) != 0) {
        block_address += UTILMEM_CFG_BYTE_ALIGN - (block_address % UTILMEM_CFG_BYTE_ALIGN);
        block_size -= 4 - (block_address % UTILMEM_CFG_BYTE_ALIGN);

        UtilLogI(__FUNCTION__, "block address has been aligned, new block_address = %p, block_size = %d", (void *)block_address, block_size);
    }
    instance->logic_block = (uint8_t *)block_address;

    /* Initialize the block_start */
    instance->block_start.next = (struct UtilMemBlock *)instance->logic_block;
    instance->block_start.size = 0;

    /* Initialize the block_end */
    block_address += block_size;
    block_address -= sc_util_mem_block_size;
    block_address &= ~(UtilPointSizeType)(UTILMEM_CFG_BYTE_ALIGN - 1);
    instance->block_end = (struct UtilMemBlock *)block_address;
    instance->block_end->size = 0;
    instance->block_end->next = NULL;

    /* block_start is the first block, its next need to point to block_end */
    instance->block_start.next->next = instance->block_end;
    instance->block_start.next->size = block_address - (UtilPointSizeType)instance->block_start.next;
    /* It's just be create, all size is the first block size */
    instance->current_size = instance->block_start.next->size;
    instance->history_min_size = instance->current_size;

    UtilLogI(__FUNCTION__, "First block address = 0x%p, size = %d", instance->block_start.next, instance->block_start.next->size);
    UtilLogI(__FUNCTION__, "Block end address = 0x%p", (void *)instance->block_end);
}

static void insertBlockToFreeList(struct UtilMemInstance *instance, struct UtilMemBlock *new_block)
{
    uint8_t *puc;

    struct UtilMemBlock *iterator = &instance->block_start;

    UtilLogI(__FUNCTION__, "new_block address = %p, size = %d", new_block, new_block->size);

    while (iterator->next < new_block) {
        iterator = iterator->next;
    }
    UtilLogI(__FUNCTION__, "iterator->next = 0x%p, size = %d", iterator->next, iterator->size);

    puc = (uint8_t *)iterator;
    /* iterator is contiguous to the new_block */
    if ((puc + iterator->size) == (uint8_t *)new_block) {
        iterator->size += new_block->size;
        new_block = iterator;
    }

    puc = (uint8_t *)new_block;
    /* new_block is contiguous to the next free block */
    if ((puc + new_block->size) == (uint8_t *)iterator->next) {
        if (iterator->next != instance->block_end) {
            new_block->size += iterator->next->size;
            new_block->next = iterator->next->next;
        }
        else {
            new_block->next = instance->block_end;
        }
    }
    else {
        new_block->next = iterator->next;
    }

    if (iterator != new_block) {
        iterator->next = new_block;
    }
}

static int8_t regionWillBeCoverd(uint8_t *block, size_t block_size)
{
	uint32_t i;
	UtilPointSizeType start_address, end_address;
	UtilPointSizeType start_exist, end_exist;
	struct UtilMemInstance *instance;
	
	int8_t flag = 0;
	
	for (i = 0; i < UTILMEM_CFG_INSTANCE_NUM; i++) {
		instance = &s_util_mem_instance[i];
		if (instance->physical_block != NULL) {
			start_address = (UtilPointSizeType)block;
			end_address = start_address + block_size;
			start_exist = (UtilPointSizeType)instance->physical_block;
			end_exist = start_exist + instance->physical_block_size;
			if (((start_address > start_exist) && (start_address < end_exist)) || ((end_address > start_exist) && (end_address < end_exist))) {
				flag = 1;
				break;
			}
		}
	}
	
	return flag;
}

#endif
